#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "pcl/point_cloud.h"
#include "pcl_ros/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/ros/conversions.h"
#include "pcl/common/transforms.h"
#include "dynamic_reconfigure/server.h"
#include <Eigen/Core>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Publisher pub_;

double min_height_(-0.95);
double max_height_(1.0);
double angle_min_(-M_PI/2);
double angle_max_(M_PI/2);
double angle_increment_(M_PI/180.0/2.0);
double scan_time_(1.0/30.0);
double range_min_(0.45);
double range_max_(10.0);
double range_min_sq_ = range_min_ * range_min_;
//output_frame_id_("/kinect_depth_frame")

PointCloud transformed;

void callback(const PointCloud::ConstPtr& cloud)
{

    sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
    output->header = cloud->header;
    output->header.frame_id = "/base_link"; // Set output frame. Point clouds come from "optical" frame, scans come from corresponding mount frame
    output->angle_min = angle_min_;
    output->angle_max = angle_max_;
    output->angle_increment = angle_increment_;
    output->time_increment = 0.0;
    output->scan_time = scan_time_;
    output->range_min = range_min_;
    output->range_max = range_max_;

    uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
    output->ranges.assign(ranges_size, output->range_max + 1.0);

    float min_y = 1000;

    for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
    {
        //KINECT ROTATED
        const float &x = it->x;
        const float &y = -it->y;
        const float &z = it->z;

        if(min_y>y) min_y=y;

        if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
        {
            //NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
            continue;
        }

        if (x > max_height_ || x < min_height_)
        {
            //NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
            continue;
        }

        double range_sq = z*z+y*y;
        if (range_sq < range_min_sq_) {
            //NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
            continue;
        }

        double angle = -atan2(-y, z);
        if (angle < output->angle_min || angle > output->angle_max)
        {
            //ros::("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
            continue;
        }
        int index = (angle - output->angle_min) / output->angle_increment;


        if (output->ranges[index] * output->ranges[index] > range_sq){
            output->ranges[index] = sqrt(range_sq);
        }
    }

//    std::cout << min_y <<std::endl;
    pub_.publish(output);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "koala_pointcloud_to_laserscan");
  ros::NodeHandle nh;
  ros::Subscriber sub_ = nh.subscribe<PointCloud>("/camera/depth_registered/points", 10, &callback);
  pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 100);
  ros::spin();
  return 0;
}
